#include "core_main.h"

/* 设备标签（全局变量） */
static WbDeviceTag lf_steer_motor, rf_steer_motor, lr_steer_motor, rr_steer_motor;     /* 舵电机 */
static WbDeviceTag lf_drive_motor, rf_drive_motor, lr_drive_motor, rr_drive_motor;     /* 轮电机 */
static WbDeviceTag lf_steer_sensor, rf_steer_sensor, lr_steer_sensor, rr_steer_sensor; /* 舵传感器 */
static WbDeviceTag lf_drive_sensor, rf_drive_sensor, lr_drive_sensor, rr_drive_sensor; /* 轮传感器 */
static WbDeviceTag imu;                                                                /* 惯性测量单元 */
// 底盘参数
float vx = 0; // x方向线速度 (m/s)
float vy = 0; // y方向线速度 (m/s)
float wz = 0; // 自转角速度 (rad/s)
double angle;
wheel_t output_wheel;

/*
 * 功能: 主程序入口
 * 参数: 无
 * 返回: 程序退出码
 */
int main()
{
    wb_robot_init();
    /* ----------------------------------------------自定义初始化----------------------------------------------- */
    /* 初始化舵电机 */
    lf_steer_motor = Motor_Init("lf_steer_motor", 0, 10);
    rf_steer_motor = Motor_Init("rf_steer_motor", 0, 10);
    lr_steer_motor = Motor_Init("lr_steer_motor", 0, 10);
    rr_steer_motor = Motor_Init("rr_steer_motor", 0, 10);

    /* 初始化轮电机 */
    lf_drive_motor = Motor_Init("lf_drive_motor", INFINITY, 0);
    rf_drive_motor = Motor_Init("rf_drive_motor", INFINITY, 0);
    lr_drive_motor = Motor_Init("lr_drive_motor", INFINITY, 0);
    rr_drive_motor = Motor_Init("rr_drive_motor", INFINITY, 0);

    /* 初始化舵电机位置传感器 */
    lf_steer_sensor = Sensor_Init("lf_steer_sensor", TIME_STEP);
    rf_steer_sensor = Sensor_Init("rf_steer_sensor", TIME_STEP);
    lr_steer_sensor = Sensor_Init("lr_steer_sensor", TIME_STEP);
    rr_steer_sensor = Sensor_Init("rr_steer_sensor", TIME_STEP);

    /* 初始化轮电机位置传感器 */
    lf_drive_sensor = Sensor_Init("lf_drive_sensor", TIME_STEP);
    rf_drive_sensor = Sensor_Init("rf_drive_sensor", TIME_STEP);
    lr_drive_sensor = Sensor_Init("lr_drive_sensor", TIME_STEP);
    rr_drive_sensor = Sensor_Init("rr_drive_sensor", TIME_STEP);

    /* 初始化IMU */
    imu = IMU_Init("IMU", TIME_STEP);

    /* 启用手柄输入 */
    wb_joystick_enable(TIME_STEP);
    wb_robot_step(100);
    /* 检测手柄输入 */
    if (wb_joystick_is_connected())
    {
        printf("手柄已连接\n");
        printf("手柄型号: %s\n", wb_joystick_get_model());
        printf("手柄轴数: %d\n", wb_joystick_get_number_of_axes());
        printf("手柄POV数: %d\n", wb_joystick_get_number_of_povs());
        wb_joystick_set_force_axis(1);
    }

    /* ----------------------------------------------主循环--------------------------------------------------- */
    while (wb_robot_step(TIME_STEP) != -1)
    {
        /* 获取IMU数据 */
        IMU_Get_XYZ_Values_Degrees(imu, NULL, NULL, &angle);
        output_wheel.angle = (float)angle;
        /* 检测手柄输入 */
        detect_joystick_input();

        if (joystick_input_1.button_c)
        {
            printf("按下C按钮\n");
            joystick_input_1.button_c = 0;
        }
        if (joystick_input_1.button_z)
        {
            printf("按下Z按钮\n");
            joystick_input_1.button_z = 0;
        }
        if (joystick_input_1.button_lb)
        {
            printf("按下LB按钮\n");
            joystick_input_1.button_lb = 0;
        }
        if (joystick_input_1.button_rb)
        {
            printf("按下RB按钮\n");
            joystick_input_1.button_rb = 0;
        }
        if (joystick_input_1.button_a)
        {
            printf("按下A按钮\n");
            joystick_input_1.button_a = 0;
        }
        if (joystick_input_1.button_b)
        {
            printf("按下B按钮\n");
            joystick_input_1.button_b = 0;
        }
        if (joystick_input_1.button_x)
        {
            printf("按下X按钮\n");
            joystick_input_1.button_x = 0;
        }
        if (joystick_input_1.button_y)
        {
            printf("按下Y按钮\n");
            joystick_input_1.button_y = 0;
        }

        if (fabs(joystick_input_1.axis_y1) > 500)
            vx = -(float)joystick_input_1.axis_y1 / 3000;
        else
            vx = 0;
        if (fabs(joystick_input_1.axis_x1) > 500)
            vy = -(float)joystick_input_1.axis_x1 / 3000;
        else
            vy = 0;
        if (joystick_input_1.lt != 0)
            wz = -0.1;
        else if (joystick_input_1.rt != 0)
            wz = 0.1;
        else
            wz = 0;
        printf("vx: %fm/s, vy: %fm/s, wz: %frad/s\n", vx, vy, wz);
        Output_Wheel(vx, vy, wz, &output_wheel);
        /* 设置舵电机角度 */
        wb_motor_set_position(lf_steer_motor, output_wheel.leftFront.direction);
        wb_motor_set_position(rf_steer_motor, output_wheel.rightFront.direction);
        wb_motor_set_position(lr_steer_motor, output_wheel.leftRear.direction);
        wb_motor_set_position(rr_steer_motor, output_wheel.rightRear.direction);
        /* 设置轮电机速度 */
        wb_motor_set_velocity(lf_drive_motor, output_wheel.leftFront.vel);
        wb_motor_set_velocity(rf_drive_motor, output_wheel.rightFront.vel);
        wb_motor_set_velocity(lr_drive_motor, output_wheel.leftRear.vel);
        wb_motor_set_velocity(rr_drive_motor, output_wheel.rightRear.vel);
    }

    /* 清理资源 */
    wb_robot_cleanup();
    return 0;
}